import launch
import launch_ros
from ament_index_python.packages import get_package_share_directory# 用于获取功能包路径
import os
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource


def generate_launch_description():
    #获取默认的urdf路径
    urdf_package_path=get_package_share_directory('dltbot_description')
    default_xacro_path=os.path.join(urdf_package_path,'urdf','dltbot/dltbot.urdf.xacro')#拼接文件路径
    default_gazebo_world_path=os.path.join(urdf_package_path,'world','custom_room.world')#拼接文件路径
    #声明一个urdf目录的一个参数,便于修改
    action_declare_arg_mode_path=launch.actions.DeclareLaunchArgument(
        name='model',default_value=str(default_xacro_path),description='加载的模型文件路径'
    )
    #通过文件路径，获取内容并转换成参数值对象，一共传入 robot_state_publisher

    substitutions_command_result=launch.substitutions.Command(['xacro ',
            launch.substitutions.LaunchConfiguration('model')])
    robot_description_value=launch_ros.parameter_descriptions.ParameterValue(substitutions_command_result,
                        value_type=str)
    #节点的启动
    action_robot_state_publisher=launch_ros.actions.Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description':robot_description_value}]
    )

    # 首先构建gazebo启动文件的路径
    gazebo_launch_file = os.path.join(
        get_package_share_directory('gazebo_ros'),
        'launch',
        'gazebo.launch.py'
    )

    # 然后正确使用IncludeLaunchDescription
    action_launch_gazebo = launch.actions.IncludeLaunchDescription(
        launch.launch_description_sources.PythonLaunchDescriptionSource(gazebo_launch_file),
        launch_arguments={'world': default_gazebo_world_path, 'verbose': 'true'}.items()
    )
    #加载机器人
    action_spawn_entity=launch_ros.actions.Node(
        package='gazebo_ros',
        executable='spawn_entity.py',
        arguments=['-topic','/robot_description','-entity','dltbot']
    )

    action_load_joint_state_controller=launch.actions.ExecuteProcess(
        cmd='ros2 control load_controller dltbot_joint_state_broadcaster --set-state active'.split(' '),
        output='screen'
    )
    
    action_load_diff_driver_controller=launch.actions.ExecuteProcess(
        cmd='ros2 control load_controller dltbot_diff_drive_controller --set-state active'.split(' '),
        output='screen'
    )

    return launch.LaunchDescription([
        action_declare_arg_mode_path,
        action_robot_state_publisher,
        action_launch_gazebo,
        action_spawn_entity,
        launch.actions.RegisterEventHandler(
            event_handler=launch.event_handlers.OnProcessExit(
                target_action=action_spawn_entity,
                on_exit=[action_load_joint_state_controller],
            )
        ),
        launch.actions.RegisterEventHandler(
            event_handler=launch.event_handlers.OnProcessExit(
                target_action=action_load_joint_state_controller,
                on_exit=[action_load_diff_driver_controller],
            )
        ),
    ])







